#include "mpu6050.h"

//////////////////////////////////////////////////////////////////////////////////
//本程序只供学习使用，未经作者许可，不得用于其它任何用途
//ALIENTEK STM32F407开发板
//MPU6050 驱动代码	   
//正点原子@ALIENTEK
//技术论坛:www.openedv.com
//创建日期:2014/5/9
//版本：V1.0
//版权所有，盗版必究。
//Copyright(C) 广州市星翼电子科技有限公司 2014-2024
//All rights reserved
//////////////////////////////////////////////////////////////////////////////////


void MPU_PIN_INIT(){
    // This example will use I2C0 on the default SDA and SCL pins (4, 5 on a Pico)
    i2c_init(MPU6050_I2C_NUM, 400*1000);
    gpio_set_function(MPU6050_I2C_PIN_SDA, GPIO_FUNC_I2C);
    gpio_set_function(MPU6050_I2C_PIN_SCL, GPIO_FUNC_I2C);
    gpio_pull_up(MPU6050_I2C_PIN_SDA);
    gpio_pull_up(MPU6050_I2C_PIN_SCL);
    // Make the I2C pins available to picotool
    bi_decl(bi_2pins_with_func(MPU6050_I2C_PIN_SDA, MPU6050_I2C_PIN_SCL, GPIO_FUNC_I2C));
}

//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Init(void) {
    uint8_t res;
    MPU_PIN_INIT();//初始化IIC总线
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80);    //复位MPU6050
    sleep_ms(100);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00);    //唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);                    //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);                    //加速度传感器,±2g
    MPU_Set_Rate(50);                        //设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG, 0X00);    //关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00);    //I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00);    //关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80);    //INT引脚低电平有效
    res = MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if (res == MPU_ADDR)//器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01);    //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00);    //加速度与陀螺仪都工作
        MPU_Set_Rate(50);                        //设置采样率为50Hz
    } else return 1;
    return 0;
}

//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr) {
    return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);//设置陀螺仪满量程范围
}

//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr) {
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);//设置加速度传感器满量程范围
}

//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_LPF(uint16_t lpf) {
    uint8_t data = 0;
    if (lpf >= 188)data = 1;
    else if (lpf >= 98)data = 2;
    else if (lpf >= 42)data = 3;
    else if (lpf >= 20)data = 4;
    else if (lpf >= 10)data = 5;
    else data = 6;
    return MPU_Write_Byte(MPU_CFG_REG, data);//设置数字低通滤波器
}

//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Rate(uint16_t rate) {
    uint8_t data;
    if (rate > 1000)rate = 1000;
    if (rate < 4)rate = 4;
    data = 1000 / rate - 1;
    data = MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);    //设置数字低通滤波器
    return MPU_Set_LPF(rate / 2);    //自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void) {
    uint8_t buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((uint16_t)buf[0] << 8) | buf[1];
    temp = 36.53 + ((double) raw) / 340;
    return temp * 100;;
}

//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx, short *gy, short *gz) {
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if (res == 0) {
        *gx = ((uint16_t)buf[0] << 8) | buf[1];
        *gy = ((uint16_t)buf[2] << 8) | buf[3];
        *gz = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;;
}

//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax, short *ay, short *az) {
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
    if (res == 0) {
        *ax = ((uint16_t)buf[0] << 8) | buf[1];
        *ay = ((uint16_t)buf[2] << 8) | buf[3];
        *az = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;;
}

//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Write_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
    uint8_t *data = malloc(len+1);
    *data = reg;
    for (int i = 0; i < len; ++i) {
        *(data + i +1) = buf[i];
    }
    i2c_write_blocking(MPU6050_I2C_NUM, addr, data, len+1, true);
    free(data);
    return 0;
}

//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
    i2c_write_blocking(MPU6050_I2C_NUM, addr, &reg, 1, true);
    i2c_read_blocking(MPU6050_I2C_NUM, addr, buf, len, false);
    return 0;
}

//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Write_Byte(uint8_t reg, uint8_t data) {
    uint8_t *buf = malloc(2);
    *buf = reg;
    *(buf+1) = data;
    i2c_write_blocking(MPU6050_I2C_NUM, MPU_ADDR, &data, 2, false);
    return 0;
}

//IIC读一个字节
//reg:寄存器地址 
//返回值:读到的数据
uint8_t MPU_Read_Byte(uint8_t reg) {
    uint8_t res;
    i2c_write_blocking(MPU6050_I2C_NUM, MPU_ADDR, &reg, 1, true);
    i2c_read_blocking(MPU6050_I2C_NUM, MPU_ADDR, &res, 1, false);
    return res;
}